#ifndef __NAV2_CUSTOM_PLANNER_HPP__
#define __NAV2_CUSTOM_PLANNER_HPP__
#include<memory>
#include<string>
#include "geometry_msgs/msg/point.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "nav2_core/global_planner.hpp"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
#include "tf2_ros/buffer.h"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_util/robot_utils.hpp"
#include "nav_msgs/msg/path.hpp"

namespace nav2_custom_planner{
class CustomPlanner : public nav2_core::GlobalPlanner{
public:
    CustomPlanner() = default;
    ~CustomPlanner() = default;

    void configure(const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent,std::string name,std::shared_ptr<tf2_ros::Buffer> tf,std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)override;

    void cleanup() override;

    void activate() override;

    void deactivate() override;

    nav_msgs::msg::Path createPlan(const geometry_msgs::msg::PoseStamped& start,const geometry_msgs::msg::PoseStamped& goal) override;

private:
    std::shared_ptr<tf2_ros::Buffer> m_tf;
    nav2_util::LifecycleNode::SharedPtr m_node;
    nav2_costmap_2d::Costmap2D * m_costmap;
    std::string m_global_frame,m_name;
    double m_interpolation_resolution;

};

}



#endif